﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using MathNet.Numerics.LinearAlgebra;

namespace Microsoft.Samples.Kinect.BodyBasics.Calculator
{
    public class InversKinematics
    {
        //矩阵参数定义
        public const double a1 = 0.01;
        public const double a2 = 0.076;
        public const double d4 = 0.107;
        public const double d6 = 0.091;

        //描述一个矩阵
        //x行
        private double mx = 1;
        private double nx = 0;
        private double px = 0;
        private double qx = 0;
        //y行
        private double my = 0;
        private double ny = 1;
        private double py = 0;
        private double qy = 0;
        //z行
        private double mz = 0;
        private double nz = 0;
        private double pz = 1;
        private double qz = 0;
        //补充行(暂时不用定义)

        public void setMatrix(Matrix<double> matrix)
        {
            mx = matrix.At(0, 0);
            nx = matrix.At(0, 1);
            px = matrix.At(0, 2);
            qx = matrix.At(0, 3);

            my = matrix.At(1, 0);
            ny = matrix.At(1, 1);
            py = matrix.At(1, 2);
            qy = matrix.At(1, 3);

            mz = matrix.At(2, 0);
            nz = matrix.At(2, 0);
            pz = matrix.At(2, 0);
            qz = matrix.At(2, 0);
        }

        public void setQVector(double[] qArray)
        {
            qx = qArray[0];
            qy = qArray[1];
            qz = qArray[2];
        }
        public void setMVector(double[] mArray)
        {
            mx = mArray[0];
            my = mArray[1];
            mz = mArray[2];
        }
        public void setNVector(double[] nArray)
        {
            nx = nArray[0];
            ny = nArray[1];
            nz = nArray[2];
        }
        public void setPVector(double[] pArray)
        {
            px = pArray[0];
            py = pArray[1];
            pz = pArray[2];
        }
        public void setQVector(float[] qArray)
        {
            //double[] qVector = new double[qArray.Length];
            //for (int i = 0; i < qArray.Length; i++)
            //{
            //    qVector[i] = (double)qArray[i];
            //}
            //setQVector(qVector);
            qx = (double)qArray[0];
            qy = (double)qArray[1];
            qz = (double)qArray[2];
        }


        /*
         需要对所有的大数据进行规约为较低的精度
         使用Math.Round()
         */
        //计算theta1
        private double theta1 = 0;
        public double cal_theta1()
        {
            //theta1 = Math.Atan((d6 * py - qy) / (d6 * px - qx));
            theta1 = atan((d6 * py - qy) / (d6 * px - qx));
            return theta1;
        }

        private double k1;
        private double k2;
        private void cal_k()
        {
            //k1 = d6 * (px * Math.Cos(theta1) + py * Math.Sin(theta1)) - (qx * Math.Cos(theta1) + qy * Math.Sin(theta1)) + a1;
            k1 = d6 * (px * cos(theta1) + py * sin(theta1)) - (qx * cos(theta1) + qy * sin(theta1)) + a1;
            k2 = d6 * pz - qz;
        }

        //计算theta2
        private double theta21;
        private double theta22;
        //使用theta22 角度
        public double cal_theta2()
        {
            //double k1 = d6 * (px * Math.Cos(theta1) + py * Math.Sin(theta1)) - (qx * Math.Cos(theta1) + qy * Math.Sin(theta1)) + a1;
            //double k2 = d6 * pz - qz;
            cal_k();

            //保留3位小数
            double x1 = Math.Round(k1, 3);
            double x2 = Math.Round(k2, 3);
            double x3 = Math.Round((d4 * d4 - k1 * k1 - k2 * k2 - a2 * a2) / (2 * a2), 3);
            //Console.WriteLine("x1:" + x1);
            //Console.WriteLine("x2:" + x2);
            //Console.WriteLine("x3:" + x3);

            //theta21 = Math.Atan((-x1 * x2 - Math.Sqrt((x1 * x1 * x3 * x3) + (x2 * x2 * x3 * x3) - Math.Pow(x3, 4)) / (x2 * x2 - x3 * x3)));
            //theta22 = Math.Atan((-x1 * x2 + Math.Sqrt(x1 * x1 * x3 * x3) + (x2 * x2 * x3 * x3) - Math.Pow(x3, 4)) / (x2 * x2 - x3 * x3));
            //double temp = Math.Round((-x1 * x2 - Math.Sqrt((x1 * x1 * x3 * x3) + (x2 * x2 * x3 * x3) - Math.Pow(x3, 4))) / (x2 * x2 - x3 * x3), 4);
            double temp0 = Math.Round(-x1 * x2, 3);
            double temp1 = Math.Round(Math.Pow(x3, 4),3);
            double temp2 = Math.Abs(Math.Round((x1 * x1 * x3 * x3) + (x2 * x2 * x3 * x3) - temp1));
            double temp3 = Math.Round((x2 * x2 - x3 * x3),3);
            double temp5 = Math.Round(temp0 + Math.Sqrt(temp2), 3);
            //当为负值时不计算,使结果为0，则delta为0，返回原点
            double judge = (x1 * x1 * x3 * x3) + (x2 * x2 * x3 * x3) - Math.Pow(x3, 4);
            if (judge <= 0)
            {
                judge = -judge;
            }
            MathNet.Numerics.Complex32 complex32 = new MathNet.Numerics.Complex32();
            
            //theta21 = atan((-x1 * x2 - Math.Sqrt((x1 * x1 * x3 * x3) + (x2 * x2 * x3 * x3) - Math.Pow(x3, 4))) / (x2 * x2 - x3 * x3));
            theta21 = atan((-x1 * x2 - Math.Sqrt(judge)) / (x2 * x2 - x3 * x3));
            //theta22 = atan((-x1 * x2 + Math.Sqrt((x1 * x1 * x3 * x3) + (x2 * x2 * x3 * x3) - Math.Pow(x3, 4))) / (x2 * x2 - x3 * x3));
            theta22 = atan((-x1 * x2 + Math.Sqrt(judge)) / (x2 * x2 - x3 * x3));
            Console.WriteLine(" temp1 init:" + temp1 + " temp2 init:" + temp2 + " temp3 init:" + temp3 + " temp5 init:" + temp5);
            //Console.WriteLine("theta22:" + theta22);
            return theta22;
        }

        //计算theta3
        private double theta31;
        private double theta32;
        //返回theta32
        public double cal_theta3()
        {
            theta31 = Math.Atan((k1 * Math.Cos(theta21) + k2 * Math.Sin(theta21) + a2) / (k1 * Math.Sin(theta21) - k2 * Math.Cos(theta21)));
            theta32 = Math.Atan((k1 * Math.Cos(theta22) + k2 * Math.Sin(theta22) + a2) / (k1 * Math.Sin(theta22) - k2 * Math.Cos(theta22)));

            return theta32;
        }

        //计算theta5
        private double theta51;
        private double theta52;
        //返回theta52
        public double cal_theta5()
        {
            theta51 = acos(-px * cos(theta1) * cos(theta21) * sin(theta31) - py * sin(theta1) * cos(theta21) * sin(theta31) - pz * sin(theta21) * sin(theta31) - px * cos(theta1) * sin(theta21) * cos(theta31) - py * sin(theta1) * sin(theta21) * cos(theta31) + pz * cos(theta21) * cos(theta31));
            theta52 = acos(-px * cos(theta1) * cos(theta22) * sin(theta32) - py * sin(theta1) * cos(theta22) * sin(theta32) - pz * sin(theta22) * sin(theta32) - px * cos(theta1) * sin(theta22) * cos(theta32) - py * sin(theta1) * sin(theta22) * cos(theta32) + pz * cos(theta22) * cos(theta32));
            return theta52;
        }

        //计算theta4
        private double theta41;
        private double theta42;
        public const double MID_theta4 = Math.PI / 3;
        public double cal_theta4()
        {
            if (sin(theta51) != 0)
            {
                theta41 = atan((-px * sin(theta1) + py * cos(theta1)) / (px * cos(theta1) * cos(theta21) * cos(theta31) + py * sin(theta1) * cos(theta21) * cos(theta31) + pz * sin(theta21) * cos(theta31) - px * cos(theta1) * sin(theta21) * sin(theta31) - py * sin(theta1) * sin(theta21) * sin(theta31) + pz * cos(theta21) * sin(theta31)));
            }
            else
            {
                theta41 = MID_theta4;
            }
            if (sin(theta52) != 0)
            {
                theta42 = atan((-px * sin(theta1) + py * cos(theta1)) / (px * cos(theta1) * cos(theta22) * cos(theta32) + py * sin(theta1) * cos(theta22) * cos(theta32) + pz * sin(theta22) * cos(theta32) - px * cos(theta1) * sin(theta22) * sin(theta32) - py * sin(theta1) * sin(theta22) * sin(theta32) + pz * cos(theta22) * sin(theta32)));
                return theta42;
            }
            else
            {
                theta42 = MID_theta4;
                return theta42;
            }

        }

        //计算theta6
        private double theta61;
        private double theta62;
        public const double MID_theta6 = Math.PI / 3;
        public double cal_theta6()
        {
            if (sin(theta41) != 0)
            {
                theta61 = atan((nx * cos(theta1) * cos(theta21) * cos(theta31) + ny * sin(theta1) * cos(theta21) * sin(theta31) + nz * sin(theta21) * sin(theta31) + nx * cos(theta1) * sin(theta21) * cos(theta31) + ny * sin(theta1) * sin(theta21) * cos(theta31) - nz * cos(theta21) * cos(theta31)) / (-mx * cos(theta1) * cos(theta21) * sin(theta31) - my * sin(theta1) * cos(theta21) * sin(theta31) - mz * sin(theta21) * sin(theta31) - mx * cos(theta1) * sin(theta21) * cos(theta31) - my * sin(theta1) * sin(theta21) * cos(theta31) + mz * cos(theta21) * cos(theta31)));
            }
            else
            {
                theta61 = MID_theta6;
            }
            if (sin(theta42) != 0)
            {
                theta62 = atan((nx * cos(theta1) * cos(theta22) * cos(theta32) + ny * sin(theta1) * cos(theta22) * sin(theta32) + nz * sin(theta22) * sin(theta32) + nx * cos(theta1) * sin(theta22) * cos(theta32) + ny * sin(theta1) * sin(theta22) * cos(theta32) - nz * cos(theta22) * cos(theta32)) / (-mx * cos(theta1) * cos(theta22) * sin(theta32) - my * sin(theta1) * cos(theta22) * sin(theta32) - mz * sin(theta22) * sin(theta32) - mx * cos(theta1) * sin(theta22) * cos(theta32) - my * sin(theta1) * sin(theta22) * cos(theta32) + mz * cos(theta22) * cos(theta32)));
                return theta62;
            }
            else
            {
                theta62 = MID_theta6;
                return theta62;
            }
        }


        public double[] calculateAll(double[] qArray_mm)
        {
            setQVector(qArray_mm);
            cal_theta1();
            cal_theta2();
            cal_theta3();
            cal_theta5();
            cal_theta4();
            cal_theta6();
            double[] result = { theta1, theta22, theta32, theta42, theta52, theta62 };
            return result;
        }
        public double[] calculateAll(Matrix<double> matrix)
        {
            setMatrix(matrix);
            cal_theta1();
            cal_theta2();
            cal_theta3();
            cal_theta5();
            cal_theta4();
            cal_theta6();
            double[] result = { theta1, theta22, theta32, theta42, theta52, theta62 };
            return result;
        }

        public double[] calculateAll_deg(double[] qArray_m)
        {
            //转换为mm
            //double[] qxyz = distance_m2mm(qArray_m);
            setQVector(qArray_m);
            cal_theta1();
            cal_theta2();
            cal_theta3();
            cal_theta5();
            cal_theta4();
            cal_theta6();
            double[] result = { theta1, theta22, theta32, theta42, theta52, theta62 };
            //转换为角度值
            Console.WriteLine("theta22:" + theta22);
            double[] degs = rad2deg(result);
            return degs;
        }


        public double[] calculateAll_deg(float[] qArray_m)
        {
            //转换为mm
            //float[] qxyz = distance_m2mm(goal_array);
            float[] qArray_transed = trans(qArray_m);
            setQVector(qArray_m);
            cal_theta1();
            cal_theta2();
            cal_theta3();
            cal_theta5();
            cal_theta4();
            cal_theta6();
            double[] result = { theta1, theta22, theta32, theta42, theta52, theta62 };
            //转换为角度值
            //Console.WriteLine("theta22:" + theta22);
            double[] degs = rad2deg(result);
            return degs;
        }
        /*PWM值在外部转换
        public double[] calculateAll_pwm(double[] qArray_m)
        {
            //转换为mm
            double[] qxyz = distance_m2mm(qArray_m);
            setQVector(qxyz);
            cal_theta1();
            cal_theta2();
            cal_theta3();
            cal_theta5();
            cal_theta4();
            cal_theta6();
            double[] result = { theta1, theta22, theta32, theta42, theta52, theta62 };
            //转换为角度值
            result = rad2deg(result);
            //转换为pwm值

            return result;
        }
        */

        //米转换为毫米
        private double[] distance_m2mm(double[] lengths)
        {
            double[] result = new double[lengths.Length];
            for (int i = 0; i < lengths.Length; i++)
            {
                result[i] = lengths[i] * 1000;
                Console.WriteLine("dis " + i + ": " + result[i]);
            }
            return result;
        }

        //弧度转换为角度
        private double[] rad2deg(double[] rads)
        {
            double[] result = new double[rads.Length];
            for (int i = 0; i < rads.Length; i++)
            {
                float rad = (float)rads[i];
                //Console.WriteLine("rad:" + rad);
                result[i] = rad * 180 / Math.PI;
                //Console.WriteLine("rad: " + "i :" + rads[i]);
                //Console.WriteLine("deg " + i + ": " + result[i]);
            }

            return result;
        }

        private float[] trans(float[] qArray)
        {
            float[] result = new float[qArray.Length];
            for(int i = 0; i < qArray.Length; i++)
            {
                result[i] = (float)(qArray[i] * 1.5);
            }
            return result;
        }

        private float[] distance_m2mm(float[] goal_array)
        {
            float[] result = new float[goal_array.Length];
            for (int i = 0; i < goal_array.Length; i++)
            {
                result[i] = goal_array[i] * 1000;
                Console.WriteLine("dis " + i + ": " + result[i]);
            }
            return result;
        }



        #region 函数重封装，方便代码改写
        private double acos(double x)
        {
            return Math.Acos(x);
        }
        private double sin(double x)
        {
            return Math.Sin(x);
        }
        private double cos(double x)
        {
            return Math.Cos(x);
        }
        private double Asin(double x)
        {
            return Math.Asin(x);
        }
        private double atan(double x)
        {
            return Math.Atan(x);
        }

        #endregion

    }
}
